#include "formation_avoidance/utils.h"


double getIntersectionTwoTriangles(Eigen::Vector2f triangle1[3],Eigen::Vector2f triangle2[3])
{
	typedef bg::model::point<double, 2, bg::cs::cartesian> point_t;

    typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > polygon;

 	// polygon polygon1{{{0.0, 0.0}, {0.0,1.0}, {1.0,1.0}, {1.0,0.0}, {0.0, 0.0}}};
 	// polygon polygon2{{{0.5, 0.0}, {0.5,1.0}, {1.5,1}, {1.5,0}, {0.5, 0.0}}};
	polygon polygon1;
	polygon polygon2;

	for(int i=0;i<3;i++)
	{
 		bg::append(polygon1.outer(), point_t(triangle1[i](0), triangle1[i](1))); 
		bg::append(polygon2.outer(), point_t(triangle2[i](0), triangle2[i](1))); 
	}
	bg::append(polygon1.outer(), point_t(triangle1[0](0), triangle1[0](1))); 
	bg::append(polygon2.outer(), point_t(triangle2[0](0), triangle2[0](1))); 


    // double area = bg::area(polygon1);
    // std::cout << "Area: " << area << std::endl;

    // double area2 = bg::area(polygon2);
    // std::cout << "Area2: " << area2 << std::endl;

    // boost::geometry::read_wkt(
    //     "POLYGON((0 0,1 0,1 1,0 1,0 0))", green);

    // boost::geometry::read_wkt(
    //     "POLYGON((0.5 0,1.5 0,1.5 1,0.5 1,0.5 0))", blue);
	// std::cout<<"ssssssss"<<std::endl;

    std::deque<polygon> output;
    boost::geometry::intersection(polygon1, polygon2, output);

    // int i = 0;
    // std::cout << "green && blue:" << std::endl;
	double overlap=0;
	// std::cout<<"ssssssss"<<std::endl;

    BOOST_FOREACH(polygon const& p, output)
    {
        // std::cout << i++ << ": " << boost::geometry::area(p) << std::endl;
		overlap+=boost::geometry::area(p);

    }

    return overlap;

}


// int main()
// {
//     typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > polygon;

//     polygon green, blue;

//  	polygon polygon1{{{0.0, 0.0}, {0.0,1.0}, {1.0,1.0}, {1.0,0.0}, {0.0, 0.0}}};
//  	polygon polygon2{{{0.5, 0.0}, {0.5,1.0}, {1.5,1}, {1.5,0}, {0.5, 0.0}}};

//     double area = bg::area(polygon1);
//     std::cout << "Area: " << area << std::endl;

//     double area2 = bg::area(polygon2);
//     std::cout << "Area2: " << area2 << std::endl;

//     boost::geometry::read_wkt(
//         "POLYGON((0 0,1 0,1 1,0 1,0 0))", green);

//     boost::geometry::read_wkt(
//         "POLYGON((0.5 0,1.5 0,1.5 1,0.5 1,0.5 0))", blue);

//     std::deque<polygon> output;
//     boost::geometry::intersection(polygon1, polygon2, output);

//     int i = 0;
//     std::cout << "green && blue:" << std::endl;
//     BOOST_FOREACH(polygon const& p, output)
//     {
//         std::cout << i++ << ": " << boost::geometry::area(p) << std::endl;
//     }


//     return 0;
// }